#include "GPUPoissonRRT.h"
#include "ompl/datastructures/NearestNeighborsGNAT.h"
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/util/Time.h"
#include <limits>
#include <iostream>
#include <fstream>
#include "PoissonSampler.h"
#include "PoissonState.h"

using namespace ompl;

namespace poisson
{
	__inline double GPUPoissonRRT::distanceFunction(const Motion* a, const Motion* b) const
	{
		return si_->distance(a->state->getState(), b->state->getState());
	}

	GPUPoissonRRT::GPUPoissonRRT(const base::SpaceInformationPtr &si, const app::GeometrySpecification* geometrySpecification) 
		: base::Planner(si, "GPUPoissonRRT"), geometrySpecification_(geometrySpecification)
	{
		specs_.approximateSolutions = true;
		specs_.directed = true;
		specs_.multithreaded = false;

		goalBias_ = 0.05;
		maxDistance_ = 0.0;
		lastGoalMotion_ = NULL;

		Planner::declareParam<double>("range", this, &GPUPoissonRRT::setRange, &GPUPoissonRRT::getRange, "0.:1.:10000.");
		Planner::declareParam<double>("goal_bias", this, &GPUPoissonRRT::setGoalBias, &GPUPoissonRRT::getGoalBias, "0.:.05:1.");
	}

	GPUPoissonRRT::~GPUPoissonRRT(void)
	{
		freeMemory();
	}

	void GPUPoissonRRT::clear(void)
	{
		Planner::clear();
		dynamic_cast<PoissonSampler*>(sampler_.get())->reset();
		GPUPoissonRRT::freeMemory();
		if (nn_)
			nn_->clear();
		lastGoalMotion_ = NULL;

	}

	void GPUPoissonRRT::setup(void)
	{
		Planner::setup();
		tools::SelfConfig sc(si_, getName());
		sc.configurePlannerRange(maxDistance_);

		if (!nn_)
			nn_.reset(new NearestNeighborsGNAT<Motion*>());
		nn_->setDistanceFunction(boost::bind(&GPUPoissonRRT::distanceFunction, this, _1, _2));

		if (!sampler_)
			sampler_ = si_->allocValidStateSampler();

		PoissonSampler* poissonSampler = dynamic_cast<PoissonSampler*>(sampler_.get());
		
		poissonSampler->setup();
	}

	void GPUPoissonRRT::freeMemory(void)
	{
		if (nn_)
		{
			std::vector<Motion*> motions;
			nn_->list(motions);
			for (unsigned int i = 0 ; i < motions.size() ; ++i)
			{
				delete motions[i];
			}
		}
	}
 
	void GPUPoissonRRT::getPlannerData(base::PlannerData &data) const
	{
		Planner::getPlannerData(data);
	}
}